#ifndef PID_CONTROLLER_H
#define PID_CONTROLLER_H

#include <stdbool.h>

#define pii 3.1415926


// 转向环 PID 控制器的全局变量（PD 控制）
extern float KP, KD;
extern float Curve_PID[3],Loc_PID[3],Locd_PID[3],Ramp_PID[3],Huan1_PID[3],Huan2_PID[3],Huan3_PID[3];
extern int turn_error, turn_lastError, turn_output;
extern int turn_setPoint, turn_outputLimit;

extern float v_dif;

extern int duoji_p,duoji_d,duoji_kd;


extern float delta_output_l;
extern float delta_output_r;
// 右轮速度环 PID 控制器的全局变量（PI 控制）
extern float speed_r[3];
extern float speed_r_w[3];
extern float speed_r_d[3];
extern int speed_i_r_output;
extern int speed_p_r_output;
extern int speed_d_r_output;
extern int speed_r_error, speed_r_lastError, speed_r_integral, speed_r_output;
extern int speed_r_setPoint, speed_r_integralLimit, speed_r_outputLimit;
extern int speed_r_integralLimit_zhi,speed_r_integralLimit_wan;
// 左轮速度环 PID 控制器的全局变量（PI 控制）
extern float speed_l[3];
extern float speed_l_w[3];
extern float speed_l_d[3];
extern int speed_l_error, speed_l_lastError, speed_l_integral, speed_l_output;
extern int speed_l_setPoint, speed_l_integralLimit, speed_l_outputLimit;
extern int speed_setPoint;
extern int speed_p_l_output;
extern int speed_i_l_output;
extern int speed_d_l_output;
extern double ackerman_left;  // 左轮差速比例
extern double ackerman_right; // 右轮差速比例
extern int speed_l_integralLimit_zhi,speed_l_integralLimit_wan;

extern float ackerman_k; // 可调半径比例

extern int wideth;
extern int delt_v;

extern float Kp,Kd,output;

void Ackerman_chasu(int duoji_pwm);
// 初始化转向环 PID 控制器（PD 控制）
void TurnPID_Init(float Kp, float Kd, int outputLimit, int setPoint);

// 计算转向环 PID 输出
float TurnPID_Calculate(int currentValue);

// 重置转向环 PID 控制器
void TurnPID_Reset();

// 初始化右轮速度环 PID 控制器（PI 控制）
void SpeedRPID_Init(float Kp, float Ki, int integralLimit, int outputLimit, int setPoint);

// 计算右轮速度环 PID 输出
float SpeedRPID_Calculate(int speed_setPoint,int currentValue);

// 重置右轮速度环 PID 控制器
void SpeedRPID_Reset();

// 初始化左轮速度环 PID 控制器（PI 控制）
void SpeedLPID_Init(float Kp, float Ki, int integralLimit, int outputLimit, int setPoint);

// 计算左轮速度环 PID 输出
float SpeedLPID_Calculate(int speed_setPoint,int currentValue);

float SpeedRPID_Calculate_wei(int speed_setPoint,int currentValue);
float SpeedLPID_Calculate_wei(int speed_setPoint,int currentValue);

// 重置左轮速度环 PID 控制器
void SpeedLPID_Reset();
void w_chasu(void);
float pid_servo_mohu(int image_error);
#endif // PID_CONTROLLER_H